#include "filter.h"
int ACC_X_BUF[ACC_FILTER_NUM], ACC_Y_BUF[ACC_FILTER_NUM], ACC_Z_BUF[ACC_FILTER_NUM]; // 滤波缓存数组
int GYRO_X_BUF[GYRO_FILTER_NUM], GYRO_Y_BUF[GYRO_FILTER_NUM], GYRO_Z_BUF[GYRO_FILTER_NUM];
void Date_Filter(void)
{
    int i;
    int temp1 = 0, temp2 = 0, temp3 = 0, temp4 = 0, temp5 = 0, temp6 = 0;

    ACC_X_BUF[0] = mpu6050_data.acc_x; // 更新滑动窗口数组
    ACC_Y_BUF[0] = mpu6050_data.acc_y;
    ACC_Z_BUF[0] = mpu6050_data.acc_z;

    GYRO_X_BUF[0] = mpu6050_data.gyro_x;
    GYRO_Y_BUF[0] = mpu6050_data.gyro_y;
    GYRO_Z_BUF[0] = mpu6050_data.gyro_z;

    for (i = 0; i < ACC_FILTER_NUM; i++)
    {
        temp1 += ACC_X_BUF[i];
        temp2 += ACC_Y_BUF[i];
        temp3 += ACC_Z_BUF[i];
    }
    for (i = 0; i < GYRO_FILTER_NUM; i++)
    {
        temp4 += GYRO_X_BUF[i];
        temp5 += GYRO_Y_BUF[i];
        temp6 += GYRO_Z_BUF[i];
    }

    mpu6050_data_lpf.acc_x = temp1 / ACC_FILTER_NUM / AcceRatio;
    mpu6050_data_lpf.acc_y = temp2 / ACC_FILTER_NUM / AcceRatio;
    mpu6050_data_lpf.acc_z = temp3 / ACC_FILTER_NUM / AcceRatio;
    mpu6050_data_lpf.gyro_x = temp4 / GYRO_FILTER_NUM / GyroRatio;
    mpu6050_data_lpf.gyro_y = temp5 / GYRO_FILTER_NUM / GyroRatio;
    mpu6050_data_lpf.gyro_z = temp6 / GYRO_FILTER_NUM / GyroRatio;

    for (i = 0; i < ACC_FILTER_NUM - 1; i++)
    {
        ACC_X_BUF[ACC_FILTER_NUM - 1 - i] = ACC_X_BUF[ACC_FILTER_NUM - 2 - i];
        ACC_Y_BUF[ACC_FILTER_NUM - 1 - i] = ACC_Y_BUF[ACC_FILTER_NUM - 2 - i];
        ACC_Z_BUF[ACC_FILTER_NUM - 1 - i] = ACC_Z_BUF[ACC_FILTER_NUM - 2 - i];
    }
    for (i = 0; i < GYRO_FILTER_NUM - 1; i++)
    {
        GYRO_X_BUF[GYRO_FILTER_NUM - 1 - i] = GYRO_X_BUF[GYRO_FILTER_NUM - 2 - i];
        GYRO_Y_BUF[GYRO_FILTER_NUM - 1 - i] = GYRO_Y_BUF[GYRO_FILTER_NUM - 2 - i];
        GYRO_Z_BUF[GYRO_FILTER_NUM - 1 - i] = GYRO_Z_BUF[GYRO_FILTER_NUM - 2 - i];
    }
}
float lpf_operator(LowPassFilter LPF, float x, float dt)
{
    float alpha = 0, y = 0;
    alpha = LPF.Tf / (LPF.Tf + dt);
    y = alpha * LPF.y_prev + (1.0f - alpha) * x;
    LPF.y_prev = y;
    return y;
}
void X_Roll_Angle_ComplementaryFilter(float G_angle, float Gyro)
{
	float fDeltaValue; // 补偿量
	static short int x_first_qidong;
	if (!x_first_qidong) // 第一次启动角度
	{
		X_Roll.g_fGyroAngleInte = G_angle;
		x_first_qidong = 1;
	}
	X_Roll.FuseAngle = X_Roll.g_fGyroAngleInte;				   // 最终融合后的角度 最终使用此值
	fDeltaValue = (G_angle - X_Roll.FuseAngle) / ROLLCONSTANT; // 时间系数矫正、补偿量
	X_Roll.g_fGyroAngleInte += (Gyro + fDeltaValue) * ROLLDT;  // 角速度积分融合后的角度
}
void Y_Pitch_Angle_ComplementaryFilter(float G_angle, float Gyro)
{
	float fDeltaValue; // 补偿量
	static short int y_first_qidong;
	if (!y_first_qidong) // 第一次启动角度
	{
		Y_Pitch.g_fGyroAngleInte = G_angle;
		y_first_qidong = 1;
	}
	Y_Pitch.FuseAngle = Y_Pitch.g_fGyroAngleInte;			   // 最终融合后的角度 最终使用此值
	fDeltaValue = (G_angle - Y_Pitch.FuseAngle) / YAWCONSTANT; // 时间系数矫正、补偿量
	Y_Pitch.g_fGyroAngleInte += (Gyro + fDeltaValue) * YAWDT;  // 角速度积分融合后的角度
}
void Z_Yaw_Angle_ComplementaryFilter(float G_angle, float Gyro)
{
	float fDeltaValue; // 补偿量
	static short int z_first_qidong;
	if (!z_first_qidong) // 第一次启动角度
	{
		Z_Yaw.g_fGyroAngleInte = G_angle;
		z_first_qidong = 1;
	}
	Z_Yaw.FuseAngle = Z_Yaw.g_fGyroAngleInte;				   // 最终融合后的角度 最终使用此值
	fDeltaValue = (G_angle - Z_Yaw.FuseAngle) / PITCHCONSTANT; // 时间系数矫正、补偿量
	Z_Yaw.g_fGyroAngleInte += (Gyro + fDeltaValue) * PITCHDT;  // 角速度积分融合后的角度
}
void imu_filter()
{
    LowPassFilter gyro_lpf_x;
    LowPassFilter gyro_lpf_y;
    LowPassFilter gyro_lpf_z;
    Date_Filter();
    mpu6050_data_lpf.gyro_x = lpf_operator(gyro_lpf_x, mpu6050_data_lpf.gyro_x, 0.001);
    mpu6050_data_lpf.gyro_y = lpf_operator(gyro_lpf_y, mpu6050_data_lpf.gyro_y, 0.001);
    mpu6050_data_lpf.gyro_z = lpf_operator(gyro_lpf_z, mpu6050_data_lpf.gyro_z, 0.001);
}